//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <bsp.h>
#include <mmu.h>
#include <init.h>
#include "flash.h"

#define FLASH_BASE          0x00000000
#define FLASH_SECTOR_SIZE   0x00040000
#define FLASH_OFFSET	    FLASH_SECTOR_SIZE

#define WRITE_MAGIC	        0x3BADBAD3

#define ERASE_UNIT_SIZE     (2 * 128 * 1024)    /*256K*/

EXTERN_C DECL_CODEINIT void CDECL WriteFlash(BootInfo *pBootInfo)
{
    kprintf("<Boot Info> (0x%x)\n"
            "LoadAddress:       0x%x\n"
            "ImageBase:         0x%x\n"
            "ImageSize:         0x%x\n"
            "MemorySize:        0x%x\n"
            "WriteMagic:        0x%x\n",
            pBootInfo,
            pBootInfo->LoadAddress,
            pBootInfo->ImageBase,
            pBootInfo->ImageSize,
            pBootInfo->MemorySize,
            pBootInfo->WriteMagic);

    if (pBootInfo->WriteMagic != WRITE_MAGIC) return;

    ulong_t ImageBase = pBootInfo->LoadAddress;
    ulong_t ImageSize = RoundUp(pBootInfo->ImageSize, 32);

    // Write netloader to first block
#define LOADER_SIZE 0x20000 // 128k

    kprintf("Warning: Write osLoader to zero flash block!(0x%x)\n",
            LOADER_SIZE);

    FlashEraseSector(FLASH_BASE, 0);
    FlashWriteBuffer(FLASH_BASE, 0, (uchar_t *)0xc0000000, LOADER_SIZE);

    //Write os image to second block
    ulong_t FlashSectorSize = FLASH_SECTOR_SIZE;
    ulong_t FlashOffset = FLASH_OFFSET;

    kprintf("Write image 0x%08x, size 0x%08x to flash (0x%08x ~ 0x%08x):\n",
            ImageBase, ImageSize,
            FlashOffset, RoundUp((FlashOffset + ImageSize), FlashSectorSize));

    for (;; FlashOffset += FlashSectorSize) {
        kprintf("Program Flash 0x%08x ...\r", FlashOffset);

        FlashEraseSector(FLASH_BASE, FlashOffset);

        ulong_t WriteSize =
            (ImageSize > FlashSectorSize)? FlashSectorSize : ImageSize;

        FlashWriteBuffer(
            FLASH_BASE, FlashOffset, (uchar_t *)ImageBase, WriteSize);

        ImageBase += WriteSize;
        ImageSize -= WriteSize;

        if (ImageSize == 0) break;
    }

//    uchar_t *src1 = (uchar_t *)FLASH_OFFSET;
//    uchar_t *src2 = (uchar_t *)0xc0010000;
//    for (uint_t i = 0; i < pBootInfo->ImageSize; i++) {
//        if (src1[i] != src2[i]) kprintf("%x vs %x\n", src1[i], src2[i]);
//    }

    kprintf("\nOK!\n");
}

/****************************************************************************
    Clear flash status register at a specific address

    Input:
    dwFlashAddr     flash address to be cleared

    Return:
    none
****************************************************************************/
void    MTFlashClearStatus(ulong_t dwFlashAddr)
{
    *((volatile ulong_t *) dwFlashAddr) = FLASH_CMD_CLEAR;
}

/****************************************************************************
    Set flash device back in read array mode

    Input:
    dwFlashAddr     flash address

    Return:
    none
****************************************************************************/
void    MTFlashSetReadArray(ulong_t dwFlashAddr)
{
    *((volatile ulong_t*)dwFlashAddr) = FLASH_CMD_READ;
}

/****************************************************************************
    Return flash status register value

    Input:
    dwFlashAddr     flash address to get status from

    Return:
    status
****************************************************************************/
ulong_t MTFlashReadStatus(ulong_t dwFlashAddr)
{
    ulong_t   status;

    *((volatile ulong_t*) dwFlashAddr) = FLASH_CMD_STATUS;
    status = *((volatile ulong_t*) dwFlashAddr);
    return status;
}

/****************************************************************************
    MTFlashBlockLockStatus(DWORD, WORD *)

    Input:
    dwFlashAddr         flash address to get lock status
    *pLockStat          pointer to hold lock status

    Return:
    RET_OK              success, lock status returned via pointer
    RET_STATUS_ERROR    error
****************************************************************************/
int MTFlashBlockLockStatus(DWORD dwFlashAddr, DWORD *pLockStat)
{
    ulong_t status;

    status = MTFlashReadStatus(dwFlashAddr);    // read flash status
    if (!STATUS_IDLE(status)) { // we can only read block lock status if device is idle
        return RET_STATUS_ERROR;
    }

    *pLockStat = (STATUS_LOCK(status)) ? MODE_LOCK : MODE_UNLOCK;
    return RET_OK;
}

/****************************************************************************
    MTFlashBlockUnlock(DWORD)

    Input:
    dwFlashAddr         flash address of block to be unlocked

    Return:
    RET_OK              success
    RET_STATUS_ERROR    error
****************************************************************************/
int MTFlashBlockUnlock(DWORD dwFlashAddr)
{
    DWORD status;
    int ret;

    status = MTFlashReadStatus(dwFlashAddr);  //  read flash status
    if (!STATUS_IDLE(status)) { //we can only lock/unlock if device is idle
        kprintf("*ERROR* invalid status when call MTFlashBlockUnlock (status: %x)\n",
                status);
        return RET_STATUS_ERROR;
    }

    *((volatile ulong_t *) dwFlashAddr) = FLASH_CMD_LOCK_SETUP; // set flash in lock setup mode
    *((volatile ulong_t *) dwFlashAddr) = FLASH_CMD_UNLOCK; // confirm unlock

    do { // todo: must implement timeout limit
        status = MTFlashReadStatus(dwFlashAddr);
    } while (STATUS_BUSY(status));

    if (STATUS_ERROR(status)) { // error bit set, need to clear it
        MTFlashClearStatus(dwFlashAddr);    // Clear error bit
        MTFlashSetReadArray(dwFlashAddr);   // set flash back in read array mode
        kprintf("*ERROR* Failed to call MTFlashBlockUnlock (status: %x)\n",
                status);
        return RET_STATUS_ERROR;
    }

    MTFlashSetReadArray(dwFlashAddr); // set flash back in read array mode

    ret = MTFlashBlockLockStatus(dwFlashAddr, &status); //read lock status
    if (ret != RET_OK) { // function fails
        kprintf("*ERROR* Failed to get lock status\n");
        return ret;
    }

    if (status == MODE_UNLOCK) {    // if block successfully unlock
//        kprintf("Unlock OK\n");
        return RET_OK;
    }
    else {  // block still locked, or locked down
        kprintf("Unlock FAILED\n");
        return RET_STATUS_ERROR;
    }
}

/****************************************************************************
    MTFlashBlockErase(ulong_t)

    Input:
    dwFlashAddr         address of block to be erased

    Return:
    RET_OK              success
    RET_STATUS_ERROR    error
****************************************************************************/
int MTFlashBlockErase(ulong_t dwFlashAddr)
{
//    kprintf("MTFlashBlockErase(%x)\n", dwFlashAddr);

    int retVal;
    ulong_t status;

    MTFlashBlockUnlock(dwFlashAddr);

    retVal = RET_OK;                            // assumed to be successful

    status = MTFlashReadStatus(dwFlashAddr);    // get block status
    if (STATUS_READY(status)) {                 // only ERASE if flash is ready
        *((volatile ulong_t *) dwFlashAddr) = FLASH_CMD_ERASE_SETUP;
        *((volatile ulong_t *) dwFlashAddr) = FLASH_CMD_CONFIRM;

        // todo: must implement timeout limit
        do {
            status = MTFlashReadStatus(dwFlashAddr);
        } while (STATUS_BUSY(status));

        for (volatile int i = 0; i < 100000; i++) {
            status = MTFlashReadStatus(dwFlashAddr);
            if (!STATUS_ERROR(status)) break;
        }
        if (STATUS_ERROR(status)) {         // error bit set, need to clear it
            MTFlashClearStatus(dwFlashAddr);    // Clear error bit
            kprintf("*ERROR* failed to call MTFlashBlockErase (status: %x)\n",
                    status);
            retVal = RET_STATUS_ERROR;
        }
    }
    else {
        kprintf("*ERROR* no ready when call MTFlashBlockErase (status: %x)\n",
                status);
        retVal = RET_STATUS_ERROR;
    }

    MTFlashSetReadArray(dwFlashAddr); // Set device back to read-array mode
    return retVal;
}


/****************************************************************************
    MTFlashWrite(ulong_t, ulong_t)

    Input:
    dwFlashAddr         flash address to be programmed
    data                data to be written to flash

    Return:
    RET_OK              success
    RET_STATUS_ERROR    error
****************************************************************************/
int MTFlashWrite(ulong_t dwFlashAddr, ulong_t data)
{
    int retVal = RET_OK; // assumed to be successful
    ulong_t status;

    status = MTFlashReadStatus(dwFlashAddr); // get block status
    if (STATUS_READY(status) || STATUS_ERASE_SUSPEND(status)) {
        *((volatile ulong_t *) dwFlashAddr) = FLASH_CMD_WRITE_SETUP;
        *((volatile ulong_t *) dwFlashAddr) = data;

        do {    // todo: must implement timeout limit
            status = MTFlashReadStatus(dwFlashAddr);
        } while (STATUS_BUSY(status));

        if (STATUS_ERROR(status)) { // error bit set, need to clear it
            MTFlashClearStatus(dwFlashAddr);    // Clear error bit
            kprintf("*ERROR* failed to call MTFlashWrite (status: %x)\n",
                    status);
            retVal =  RET_STATUS_ERROR;
        }
    }
    else {
        kprintf("*ERROR* no ready when call MTFlashWrite (status: %x)\n",
                status);
        retVal = RET_STATUS_ERROR;
    }

    MTFlashSetReadArray(dwFlashAddr);             /* Set device back to read-array mode */
    return retVal;
}

////////////////////////////////////////////////////////////////////////////
//****************************************************************************
//
// Erases a specific sector of the FLASH memory.
//
//****************************************************************************
int MTFlashEraseSector(ulong_t ulFlashBase, long lSectorOffset)
{
    ulong_t flashaddr = (ulong_t)(ulFlashBase + lSectorOffset);;

    MTFlashBlockErase(flashaddr);

    return 0;
}

int MTFlashReadBuffer(ulong_t FlashBase, long Offset, uchar_t *Buffer, long BufferSize)
{
    long i;
    ulong_t * flashaddr = (ulong_t *)(FlashBase + Offset);
    ulong_t * memoryptr = (ulong_t *)Buffer;

    if ((BufferSize & 0x3) != 0) { //must align with 4
        kprintf("*ERROR* invalid buffer size: 0x%x\n", BufferSize);
        return 1;
    }

    MTFlashSetReadArray((ulong_t)flashaddr);

    for (i = 0; i < BufferSize; i += 4 ) {
        *memoryptr++ = *flashaddr++;
    }

    return 0;
}


//****************************************************************************
//
// Writes a block of data into the FLASH memory.
//
//****************************************************************************
int MTFlashWriteBuffer (ulong_t FlashBase, long Offset, uchar_t *Buffer, long BufferSize)
{
    int i, ret;
    long BufSizeWritten = 0;
    ulong_t * flashaddr = (ulong_t *)(FlashBase + Offset);
    ulong_t * memoryptr = (ulong_t *)Buffer;

//    assert(BufferSize <= 512);//ftl min block size
    assert((BufferSize & 0x1) == 0);//must align with 2

    for (; BufSizeWritten < (BufferSize & ~(ERASE_UNIT_SIZE-1));
        BufSizeWritten += ERASE_UNIT_SIZE) {
        for (i = 0; i < ERASE_UNIT_SIZE / 4; i++) {
            ret = MTFlashWrite((ulong_t)flashaddr++, *memoryptr++);
        }
    }

    for (; BufSizeWritten < (BufferSize & ~0x3); BufSizeWritten += 4) {
        ret = MTFlashWrite((ulong_t)flashaddr++, *memoryptr++);
    }

    if (BufSizeWritten < BufferSize) {
        //last two bytes
        ulong_t addr = (ulong_t )flashaddr & ~3;
        ulong_t data = *(ulong_t *)addr;
        if ((ulong_t)flashaddr & 0x3) {//align with 4
            ret = MTFlashWrite(addr,
                (data & 0x0000ffff) | (*(unsigned short*)memoryptr << 16));
        }
        else {
            ret = MTFlashWrite(addr,
                ( data & 0xffff0000) | *(unsigned short*)memoryptr);
        }
        BufSizeWritten += 2;
    }

    return 0;
}
